This invention relates to tracking and control of tiltable bodies.
It is known to use quaternions to represent the orientation in space of an object. Quaternion notation is, in general, more computationally efficient to use than the more widely employed Euler data representation. Furthermore, quaternion notation is not subject to singularities which can occur when using Euler notation. The following U.S. patents disclose using quaternions to control, determine and/or display the orientation of an object in space: Nos. 5,875,993; 5,212,480; 4,797,836; 4,742,356; and 4,737,794.
A quaternion is a four-element, hypercomplex number first conceived by Sir William Rowan Hamilton in 1843. A quaternion consists of a scalar part and a complex vector part. The vector part consists of an ordered triple (vector) of three real components which are assigned direction by three orthogonal complex unit vectors: i, j, k. An example of a general quaternion Q is shown below:
Q=q0+iq1+jq2+kq3xe2x80x83xe2x80x83(Eq. 1)
Addition of quaternions is performed by adding components in like directions. Multiplication is performed by noting the following products of the unit basis vectors:
i2=j2=k2=ijk=xe2x88x921xe2x80x83xe2x80x83(Eq. 2)
ij=xe2x88x92ji=kxe2x80x83xe2x80x83(Eq. 3)
jk=xe2x88x92kj=ixe2x80x83xe2x80x83(Eq. 4)
ki=xe2x88x92ik=jxe2x80x83xe2x80x83(Eq. 5)
Since the quaternion is hypercomplex, it also has a complex conjugate in which the direction of the vector part is reversed. An example is shown below:
Q*=q0xe2x88x92iq1xe2x88x92jq2xe2x88x92kq3xe2x80x83xe2x80x83(Eq. 6)
The square magnitude of the quaternion can be computed by forming the product of the quaternion with its complex conjugate as shown:
Q2=QQ*=q02+q12+q22+q32xe2x80x83xe2x80x83(Eq. 7)
A quaternion with unit magnitude (Q2=1) has special significance. Specifically, it acts as a two-sided rotation operator. It is worthy of note that Hamilton discovered quaternions through his efforts to develop a three dimensional extension of the rotational effect produced in the complex plane when a complex number is multiplied by a unit complex number of the form exp (ixcex8). The rotational effect of exp (ixcex8) results because multiplication of complex numbers requires the multiplication of their respective magnitudes and the addition of their respective phases. Since exp (ixcex8) has unit magnitude, it can only affect the phase of the product. In the complex plane, this manifests itself as a rotation about the origin by an angle xcex8. In trying to generalize this effect to vector rotations, Hamilton originally tried three-element hypercomplex numbers. It was not until he realized that four elements are required to account for xe2x80x9cphasexe2x80x9d changes in three-dimensional space that he successfully produced the desired result.
Typically, a vector rotation is accomplished using a one-sided rotation operator, R, which in three-dimensional space can be represented as a real, 3xc3x973 orthogonal matrix. This transformation matrix rotates a vector, x, into a vector, xxe2x80x2, by multiplication on the left as shown:
xxe2x80x2=Rx where x xcex5 R3xc3x971 and R xcex5 R3xc3x973xe2x80x83xe2x80x83(Eq. 8)
A two-sided operator must be applied using both left and right multiplication. In the case of the quaternion operator, the rotation is accomplished when a given quaternion (X) with a null scalar part (i.e. a vector) is pre- and post-multiplied by the unit quaternion and its conjugate as shown:
Xxe2x80x2=QXQ*xe2x80x83xe2x80x83(Eq. 9)
The resulting vector, Xxe2x80x2, is rotated about a general axis by a specific angle, both of which are determined by the unit quaternion, Q. If the axis of rotation is denoted by a unit vector, n, and the angle of rotation is denoted by an angle, xcex8, then the unit quaternion components can be written as:
q0=cos(xcex8/2) and q=nsin(xcex8/2) where q=(q1, q2,q3)xe2x80x83xe2x80x83(Eq. 10)
These components satisfy the normalization condition:
1=q02+q12+q22+q32xe2x80x83xe2x80x83(Eq. 11)
The quaternion components thus defined are also called the Euler Parameters. These parameters contain all of the information necessary to derive the axis and angle of rotation. The rotation axis defined by unit vector, n, is also called the eigenaxis since it is the eigenvector of the one-sided rotation matrix, R, corresponding to the eigenvalue, xcex=+1. This occurs because the axis of rotation must be common to both the original and rotated frames and therefore must be unchanged by the rotation operator. Note that the so-called eigenaxis rotation is a single rotation about a general axis as compared to the Euler angle rotation which accomplishes the same transformation by performing three separate rotations: yaw, pitch, and roll, about the z, y, and x axes, respectively.
The quaternion complex unit vectors (i, j, k) are related to the Pauli spin matrices as shown:
i=xe2x88x92i"sgr"1xe2x80x83xe2x80x83(Eq. 12)
j=xe2x88x92i"sgr"2xe2x80x83xe2x80x83(Eq. 13)
k=xe2x88x92i"sgr"3xe2x80x83xe2x80x83(Eq. 13A)
where                     ⅈ        =                              -            1                                              (                  Eq          .                      xe2x80x83                    ⁢          14                )                                          σ          0                =                  [                                                    1                                            0                                                                    0                                            1                                              ]                                    (                  Eq          .                      xe2x80x83                    ⁢          15                )                                          σ          1                =                  [                                                    0                                            1                                                                    1                                            0                                              ]                                    (                  Eq          .                      xe2x80x83                    ⁢          16                )                                          σ          2                =                  [                                                    0                                                              -                  ⅈ                                                                                    ⅈ                                            0                                              ]                                    (                  Eq          .                      xe2x80x83                    ⁢          17                )                                          σ          3                =                  [                                                    1                                            0                                                                    0                                                              -                  1                                                              ]                                    (                  Eq          .                      xe2x80x83                    ⁢          18                )            
(See The Theory of Spinors by E. Cartan.)
Using the definitions of the Pauli spin matrices and the unit quaternion coefficients, the unit quaternion can be written as:
Q="sgr"0cos(xcex8/2)xe2x88x92i(nxc2x7"sgr")sin(xcex8/2)xe2x80x83xe2x80x83(Eq. 19)
where
nxc2x7"sgr"=n1"sgr"1+n2"sgr"2+n3"sgr"3xe2x80x83xe2x80x83(Eq. 20)
This can also be shown to be equivalent to the following matrix exponential:
Q=exe2x88x92i"sgr"xc2x7xcex8/2 where xcex8=nxcex8xe2x80x83xe2x80x83(Eq. 21)
Note the similarity between this form of the quaternion and the exponential form of a unit complex number discussed previously. This form suggests the three-dimensional change in xe2x80x9cphasexe2x80x9d that Hamilton was originally seeking. The appearance of the half angle is accounted for by the fact that Q is a two-sided transformation. Thus the left and right factors, Q and Q*, each contribute half of the desired spatial phase shift.
It is often more convenient to use the Pauli spin matrix form rather than the traditional Hamiltonian form of the quaternion. For example, a vector can be represented as a matrix by forming the inner product as shown:
xe2x80x83X=xxc2x7"sgr"=x1"sgr"1+x2"sgr"2+x3"sgr"3xe2x80x83xe2x80x83(Eq. 22)
which yields:                     X        =                  [                                                                      x                  3                                                                                                  x                    1                                    -                                      ⅈ                    ⁢                                          xe2x80x83                                        ⁢                                          x                      2                                                                                                                                                                x                    1                                    +                                      ⅈ                    ⁢                                          xe2x80x83                                        ⁢                                          x                      2                                                                                                                    -                                      x                    3                                                                                ]                                    (                  Eq          .                      xe2x80x83                    ⁢          23                )            
This matrix form has many useful properties. For example, it can be shown that the reflection of the vector, x, through a plane defined by the unit normal, a, is easily produced using the matrix form of the vectors as shown:
Xxe2x80x2=xe2x88x92AXA where A=axc2x7"sgr"xe2x80x83xe2x80x83(Eq. 24)
It can also be shown that any rotation can be produced by two reflections. If the planes of reflection intersect at an angle of xcex8/2 and the line of intersection is defined by the unit vector, n, then the resulting transformation will rotate any vector, x, about the eigenaxis, n, by an angle, xcex8. This is illustrated below where the unit normals to the planes are vectors a and b.
Xxe2x80x2=BAXABxe2x80x83xe2x80x83(Eq. 25)
This two-sided operator, which performs a rotation, closely resembles the quaternion rotation described previously. In fact, it can be shown that Q=BA. The following multiplicative identity results from the properties of the matrix form of a vector:
AB="sgr"0axc2x7b+i(axc3x97b)xc2x7"sgr"xe2x80x83xe2x80x83(Eq. 26)
In the case of a rotation by angle xcex8,the unit normal vectors, a and b, must intersect at angle xcex8/2. Therefore, their dot and cross products produce axc2x7b=cos(xcex8/2) and axc3x97b=nsin xcex8/2) where n is parallel to the line of intersection. Substitution of these values into Eq. 26 produces the desired result:
Q=BA="sgr"0cos(xcex8/2)xe2x88x92i(nxc2x7"sgr")sin(xcex8/2)=exe2x88x92i"sgr"xc2x7xcex8/2xe2x80x83xe2x80x83(Eq. 27)
An interesting feature of this development is that the coefficients of the quaternion can be found using simple dot and cross products of the appropriate unit vectors. In particular, the two vectors, a and b, must be perpendicular to the axis of rotation and separated by an angle of xcex8/2. It can also be shown that given knowledge of two arbitrary vectors xcex1 and xcex2 in one frame of reference and knowledge of their rotated counterparts xcex1xe2x80x2 and xcex2xe2x80x2 in the rotated frame of reference, then the quaternion and the associated rotation matrix responsible for the transformation can be uniquely determined. Thus, given two vectors defined in the base frame between the origin and two external reference points, the attitude of the system can be uniquely determined by taking additional xe2x80x9csightingsxe2x80x9d of the same two external reference points from the rotated frame and comparing the coordinates of the reference vectors measured in the final frame to those measured in the initial frame.
It may also be contemplated to compute quaternion components by direct integration of a quaternion rate. This requires an expression for the derivative of the quaternion as a function of angular rate. This can be derived by differentiating Eq. 21. The resulting derivatives of the quaternion coefficients are each a linear combination of the angular rate components weighted by the existing quaternion components. If the quaternion and angular rates are placed in vector form, the following matrix equation results:                               [                                                                                          q                    .                                    0                                                                                                                          q                    .                                    1                                                                                                                          q                    .                                    2                                                                                                                          q                    .                                    3                                                              ]                =                                            1              2                        ⁢                          xe2x80x83                        [                                                                                -                                          q                      1                                                                                                            -                                          q                      2                                                                                                            -                                          q                      3                                                                                                                                        q                    0                                                                                        -                                          q                      3                                                                                                            q                    2                                                                                                                    q                    3                                                                                        q                    0                                                                                        -                                          q                      1                                                                                                                                        -                                          q                      2                                                                                                            q                    1                                                                                        q                    0                                                                        ]                    ⁢                      xe2x80x83                    [                                                                      ω                  1                                                                                                      ω                  2                                                                                                      ω                  3                                                              ]                                    (                  Eq          .                      xe2x80x83                    ⁢          28                )            
where the dotted qi are the components of the quaternion rate and the xcfx89i are the components of the angular rate xcfx89 (={dot over (xcex8)}).
The orientation and/or rotation of a body may be represented in more than one frame of reference. For example it is possible to define a frame of reference relative to the body of interest itself, or to define a frame of reference relative to an external fixed object. For some applications the earth may be taken as a fixed object for defining a frame of reference. In Eq. 28 the angular rate is referenced to the body frame. A similar matrix equation in which the angular rate is referenced to the earth frame is shown below as Eq. 29. Note that shifting the reference frame produces several sign changes in the quaternion matrix.                               [                                                                                          q                    .                                    0                                                                                                                          q                    .                                    1                                                                                                                          q                    .                                    2                                                                                                                          q                    .                                    3                                                              ]                =                                            1              2                        ⁢                          xe2x80x83                        [                                                                                -                                          q                      1                                                                                                            -                                          q                      2                                                                                                            -                                          q                      3                                                                                                                                        q                    0                                                                                        q                    3                                                                                        -                                          q                      2                                                                                                                                        -                                          q                      3                                                                                                            q                    0                                                                                        q                    1                                                                                                                    q                    2                                                                                        -                                          q                      1                                                                                                            q                    0                                                                        ]                    ⁢                      xe2x80x83                    [                                                                      ω                  E1                                                                                                      ω                  E2                                                                                                      ω                  E3                                                              ]                                    (                  Eq          .                      xe2x80x83                    ⁢          29                )            
Eq. 29 can be written in a more compact form using matrix variables as shown:
dq/dt=Qxcfx89E/2xe2x80x83xe2x80x83(Eq. 30)
Eq. 30 can also be solved for xcfx89E as shown below by noting that QTQ=I:
xcfx89E=2QT(dq/dt)xe2x80x83xe2x80x83(Eq. 31)
When the quaternion rate equation is integrated using the instantaneous angular rate of the body in question, the resulting quaternion contains the information necessary to transform coordinates from the initial frame of reference to the rotated frame. Thus, the quaternion components can be used to form the rotation matrix, R, described previously (Eq. 8). The rotation matrix which transforms body coordinates into earth coordinates is shown below in terms of quaternion components:                               R          EB                =                  [                                                                                          q                    0                    2                                    +                                      q                    1                    2                                    -                                      q                    2                    2                                    -                                      q                    3                    2                                                                                                2                  ⁢                                      (                                                                                            q                          1                                                ⁢                                                  q                          2                                                                    -                                                                        q                          3                                                ⁢                                                  q                          0                                                                                      )                                                                                                2                  ⁢                                      (                                                                                            q                          1                                                ⁢                                                  q                          3                                                                    +                                                                        q                          2                                                ⁢                                                  q                          0                                                                                      )                                                                                                                        2                  ⁢                                      (                                                                                            q                          2                                                ⁢                                                  q                          1                                                                    +                                                                        q                          3                                                ⁢                                                  q                          0                                                                                      )                                                                                                                    q                    0                    2                                    -                                      q                    1                    2                                    +                                      q                    2                    2                                    -                                      q                    3                    2                                                                                                2                  ⁢                                      (                                                                                            q                          2                                                ⁢                                                  q                          3                                                                    -                                                                        q                          1                                                ⁢                                                  q                          0                                                                                      )                                                                                                                        2                  ⁢                                      (                                                                                            q                          3                                                ⁢                                                  q                          1                                                                    -                                                                        q                          2                                                ⁢                                                  q                          0                                                                                      )                                                                                                2                  ⁢                                      (                                                                                            q                          3                                                ⁢                                                  q                          2                                                                    +                                                                        q                          1                                                ⁢                                                  q                          0                                                                                      )                                                                                                                    q                    0                    2                                    -                                      q                    1                    2                                    -                                      q                    2                    2                                    +                                      q                    3                    2                                                                                ]                                    (                  Eq          .                      xe2x80x83                    ⁢          32                )            
Since the transformation matrix, REB, is orthogonal, its transpose is the inverse transformation, RBE. Therefore, quaternions can be used to track the attitude of a body through any angular motion.
For the purpose of comparison, the equations for computing the Euler angle rates are shown below. The associated rotation matrix, R, is also shown as a function of the Euler angles.
dxcfx86/dt=P+(d"psgr"/dt)sinxcex8
dxcex8/dt=Qcosxcfx86xe2x88x92Rsinxcfx86
d"psgr"/dt=(Qsinxcfx86+Rcosxcfx86)secxcex8                                          where            ⁢                          xe2x80x83                        ⁢            ϕ                    =                      xe2x80x83                    ⁢          Roll                                              θ          =                      xe2x80x83                    ⁢          Pitch                                              ψ          =                      xe2x80x83                    ⁢          Yaw                                              ω          =                      xe2x80x83                    ⁢                                    Angular              ⁢                              xe2x80x83                            ⁢              Rate              ⁢                              xe2x80x83                            ⁢              Vector                        =                                          [                                                                            P                                                              Q                                                              R                                                                      ]                            T                                                      R      EB        =          [                                                  cos              ⁢                              xe2x80x83                            ⁢              ψ              ⁢                              xe2x80x83                            ⁢              cos              ⁢                              xe2x80x83                            ⁢              θ                                                                          cos                ⁢                                  xe2x80x83                                ⁢                ψ                ⁢                                  xe2x80x83                                ⁢                sin                ⁢                                  xe2x80x83                                ⁢                θ                ⁢                                  xe2x80x83                                ⁢                sin                ⁢                                  xe2x80x83                                ⁢                φ                            -                              sin                ⁢                                  xe2x80x83                                ⁢                ψ                ⁢                                  xe2x80x83                                ⁢                cos                ⁢                                  xe2x80x83                                ⁢                φ                                                                                        cos                ⁢                                  xe2x80x83                                ⁢                ψ                ⁢                                  xe2x80x83                                ⁢                sin                ⁢                                  xe2x80x83                                ⁢                θ                ⁢                                  xe2x80x83                                ⁢                cos                ⁢                                  xe2x80x83                                ⁢                φ                            +                              sin                ⁢                                  xe2x80x83                                ⁢                ψ                ⁢                                  xe2x80x83                                ⁢                sin                ⁢                                  xe2x80x83                                ⁢                φ                                                                                        sin              ⁢                              xe2x80x83                            ⁢              ψ              ⁢                              xe2x80x83                            ⁢              cos              ⁢                              xe2x80x83                            ⁢              θ                                                                          sin                ⁢                                  xe2x80x83                                ⁢                ψ                ⁢                                  xe2x80x83                                ⁢                sin                ⁢                                  xe2x80x83                                ⁢                θ                ⁢                                  xe2x80x83                                ⁢                sin                ⁢                                  xe2x80x83                                ⁢                φ                            +                              cos                ⁢                                  xe2x80x83                                ⁢                ψ                ⁢                                  xe2x80x83                                ⁢                cos                ⁢                                  xe2x80x83                                ⁢                φ                                                                                        sin                ⁢                                  xe2x80x83                                ⁢                ψ                ⁢                                  xe2x80x83                                ⁢                sin                ⁢                                  xe2x80x83                                ⁢                θ                ⁢                                  xe2x80x83                                ⁢                cos                ⁢                                  xe2x80x83                                ⁢                φ                            -                              cos                ⁢                                  xe2x80x83                                ⁢                ψ                ⁢                                  xe2x80x83                                ⁢                sin                ⁢                                  xe2x80x83                                ⁢                φ                                                                                                        -                sin                            ⁢                              xe2x80x83                            ⁢              θ                                                          cos              ⁢                              xe2x80x83                            ⁢              θ              ⁢                              xe2x80x83                            ⁢              sin              ⁢                              xe2x80x83                            ⁢              φ                                                          cos              ⁢                              xe2x80x83                            ⁢              θ              ⁢                              xe2x80x83                            ⁢              cos              ⁢                              xe2x80x83                            ⁢              φ                                          ]      
As is known, the Euler method depends heavily on trigonometric functions and is quite computationally intensive. In addition, the Euler rate equations depend on the secant of the pitch angle which results in a singularity when the pitch reaches xc2x190 degrees. In contrast, the quaternion method contains no trigonometric functions and relies only on multiplication and addition. Consequently, the quaternion method is far more computationally efficient. As was noted above, the attitude of a body can be uniquely represented in quaternion form, given two vectors defined in the body frame and two external reference points. However, it can be difficult and/or expensive to establish two external reference points, particularly in the case of terrestrial objects, whether located on land or on or under the surface of a body of water.
The present inventor believes he is the first to recognize how quaternion notation may be usefully modified such that only one external reference point is needed to generate the modified quaternion form, and that such a modified quaternion form may be usefully applied to bodies which are free to tilt, but for which the heading (yaw) is constrained or need not be controlled by the system which employs the modified quaternions.
It is an object of the invention to provide improved tracking and/or control of tiltable objects.
It is a further object of the invention to provide a tracking and control system which is less computationally demanding than prior art systems.
It is still a further object of the invention to provide quaternion processing to a system in which only one external reference is available.
It is yet another object of the invention to provide reliable high-bandwidth tracking of an object""s orientation.
An aspect of the invention provides a method of estimating the attitude of a tiltable body, where the body includes tilt sensing devices such as a fluidic tilt sensors and angular rate sensing devices such as gyros. The method includes the steps of: outputting angular rate information from the angular rate sensing devices, transforming and integrating the outputted angular rate information to produce first quaternion position information such that the first quaternion position information is constrained to represent a rotation about a horizontal axis in an earth frame of reference, outputting tilt information from the tilt sensing devices, processing the outputted tilt information to generate second quaternion position information such that the second quaternion position information is constrained to represent a rotation about a horizontal axis in the earth frame of reference, comparing the first quaternion position information to generate error information, and using the error information to compensate for drift in the angular rate sensing devices.
Another aspect of the invention provides a method of estimating the attitude of a tiltable body, where the body includes tilt sensing devices such as fluidic tilt sensors and angular rate sensing devices such as gyros. The method includes the steps of: outputting angular rate information from the angular rate sensing devices, generating modified quaternion rate information from the outputted rate information, with the modified quaternion rate information being in the form of a scalar quantity and a two-component vector, integrating the modified quaternion rate information to generate first modified quaternion attitude information, with the step of generating the modified quaternion rate information being constrained so that the first modified quaternion attitude information is of the form e=e0+e1i+e2j, where i, j are imaginary unit vectors which satisfy the following relations with each other and with a third imaginary unit vector k:
i2=j2=k2=ijk=xe2x88x921, ij=xe2x88x92ji=k, jk=xe2x88x92kj=i, ki=xe2x88x92ik=j;
outputting tilt information from the tilt sensing devices, generating second modified quaternion position information from the outputted tilt information, with the second quaternion position information being of the form e=e0+e1i+e2j, comparing the first modified quaternion position information to the second modified quaternion position information to generate modified quaternion position error information, transforming the modified quaternion position error information into an angular rate error signal, and using the angular rate error signal to compensate for an error in the outputted angular rate information.
With the method provided in accordance with the invention, tilt information outputted from a fluidic sensor or the like is used to compensate for drift or other offsets in a high-bandwidth angular rate sensor, such as a set of gyros. The error compensation provided through the tilt reference data allows the corrected high-bandwidth angular rate data to be integrated to provide a real-time attitude tracking estimate that does not accumulate offset errors. Heading angular rate information (yaw rate information) is discarded to produce a modified (3-component) quaternion representation that is compared with a similar modified quaternion rate information derived from tilt information and based on a single, gravitational reference point. Error compensation and filtering are performed with quaternion notation to provide computational efficiency.
The above, and other aspects, features and advantages of the invention will be set forth in and/or apparent from the following description and drawings.